/*
 * Robot Arm Demo using EASML Enhanced Library
 * 
 * This example demonstrates how to use the EASML math library
 * combined with robot arm control functionality
 */

#include "common.h"
#include "rob.h"
#include "sensor.h"
#include "protocol.h"
#include "mfl.h"
#include "crc.h"

/* System configuration */
#define DEMO_CONTROL_PERIOD_MS 20u
#define DEMO_LINK_L0 85.0f    /* Base height: 85mm */
#define DEMO_LINK_L1 105.0f   /* Upper arm: 105mm */
#define DEMO_LINK_L2 98.0f    /* Forearm: 98mm */
#define DEMO_LINK_L3 90.0f    /* End effector: 90mm */

/* Global system state */
static Rob_SystemState_Type g_RobotState;
static Sensor_System_Type g_SensorSystem;
static Protocol_Context_Type g_ProtocolContext;

/* Function prototypes */
static void Demo_SystemInit(void);
static void Demo_BasicMovement(void);
static void Demo_SensorInteraction(void);
static void Demo_CommunicationTest(void);
static void Demo_MathLibraryTest(void);
static void Demo_ControlLoop(void);

/*
 * Main demonstration function
 */
int main(void)
{
    uint32 system_tick = 0u;
    uint32 last_control_time = 0u;
    
    /* Initialize all systems */
    Demo_SystemInit();
    
    printf("EASML Robot Arm Demo Started\n");
    printf("=============================\n\n");
    
    /* Run demonstration sequence */
    Demo_MathLibraryTest();
    Demo_BasicMovement();
    Demo_SensorInteraction();
    Demo_CommunicationTest();
    
    printf("Starting continuous control loop...\n");
    
    /* Main control loop */
    while (1)
    {
        system_tick++;
        
        /* Control loop timing */
        if ((system_tick - last_control_time) >= DEMO_CONTROL_PERIOD_MS)
        {
            Demo_ControlLoop();
            last_control_time = system_tick;
        }
        
        /* Simulate 1ms delay */
        /* In real system, this would be hardware timer interrupt */
        /* delay_ms(1); */
        
        /* Break after some time for demo purposes */
        if (system_tick > 10000u)
        {
            break;
        }
    }
    
    printf("Demo completed successfully!\n");
    
    return 0;
}

/*
 * Initialize all system components
 */
static void Demo_SystemInit(void)
{
    uint8 i;
    
    printf("Initializing robot arm system...\n");
    
    /* Initialize robot arm with link dimensions */
    Rob_Init(&g_RobotState, DEMO_LINK_L0, DEMO_LINK_L1, DEMO_LINK_L2, DEMO_LINK_L3);
    
    /* Configure PID controllers for each servo */
    for (i = 0u; i < ROB_SERVO_NUM; i++)
    {
        Rob_InitPID(&g_RobotState, i, 2.0f, 0.1f, 0.05f);  /* Kp=2.0, Ki=0.1, Kd=0.05 */
    }
    
    /* Initialize sensor system */
    Sensor_Init(&g_SensorSystem);
    
    /* Initialize communication protocol */
    Protocol_Init(&g_ProtocolContext);
    
    printf("System initialization complete.\n\n");
}

/*
 * Demonstrate basic robot arm movements using inverse kinematics
 */
static void Demo_BasicMovement(void)
{
    uint8 result;
    uint8 i;
    
    printf("=== Basic Movement Demonstration ===\n");
    
    /* Home position */
    printf("Moving to home position...\n");
    result = Rob_InverseKinematics(0.0f, 200.0f, 150.0f, 0.0f, &g_RobotState);
    if (result == 0u)
    {
        printf("Home position calculated successfully:\n");
        for (i = 0u; i < ROB_DOF; i++)
        {
            printf("  Joint %d: %.2f degrees\n", i, g_RobotState.TargetAngles.Theta[i]);
        }
        Rob_MoveToPosition(&g_RobotState, 0.0f, 200.0f, 150.0f, 0.0f, 2000u);
    }
    else
    {
        printf("Failed to calculate home position (error: %d)\n", result);
    }
    
    /* Move to several positions */
    float32 test_positions[][4] = {
        {50.0f, 150.0f, 100.0f, -30.0f},   /* Position 1 */
        {-50.0f, 180.0f, 120.0f, 45.0f},  /* Position 2 */
        {0.0f, 120.0f, 180.0f, 0.0f},     /* Position 3 */
        {30.0f, 220.0f, 90.0f, -45.0f}    /* Position 4 */
    };
    
    for (i = 0u; i < 4u; i++)
    {
        printf("\nMoving to position %d: (%.1f, %.1f, %.1f) α=%.1f°...\n",
               i + 1, test_positions[i][0], test_positions[i][1],
               test_positions[i][2], test_positions[i][3]);
        
        result = Rob_InverseKinematics(test_positions[i][0], test_positions[i][1],
                                      test_positions[i][2], test_positions[i][3],
                                      &g_RobotState);
        
        if (result == 0u)
        {
            printf("  Solution found - executing movement\n");
            Rob_MoveToPosition(&g_RobotState, test_positions[i][0], test_positions[i][1],
                              test_positions[i][2], test_positions[i][3], 1500u);
            
            /* Simulate movement time */
            /* delay_ms(1500); */
        }
        else
        {
            printf("  No solution found (error: %d)\n", result);
        }
    }
    
    printf("\nBasic movement demonstration complete.\n\n");
}

/*
 * Demonstrate sensor integration
 */
static void Demo_SensorInteraction(void)
{
    float32 ultrasonic_distance;
    float32 ir_distance;
    uint8 red, green, blue;
    boolean touch_state;
    
    printf("=== Sensor Interaction Demonstration ===\n");
    
    /* Initialize sensors */
    Sensor_UltrasonicInit(&g_SensorSystem.Ultrasonic, 10u, 11u, 3000.0f);
    Sensor_InfraredInit(&g_SensorSystem.Infrared, 0u);
    Sensor_TouchInit(&g_SensorSystem.Touch, 12u);
    Sensor_ColorInit(&g_SensorSystem.Color, 0x29u);
    
    /* Simulate sensor readings */
    printf("Reading sensors...\n");
    
    /* Ultrasonic sensor */
    Sensor_UltrasonicRead(&g_SensorSystem.Ultrasonic);
    ultrasonic_distance = Sensor_UltrasonicGetDistance(&g_SensorSystem.Ultrasonic);
    if (ultrasonic_distance > 0.0f)
    {
        printf("  Ultrasonic distance: %.1f mm\n", ultrasonic_distance);
        
        /* React to obstacle detection */
        if (ultrasonic_distance < 100.0f)
        {
            printf("  Obstacle detected! Moving arm to safe position.\n");
            Rob_MoveToPosition(&g_RobotState, 0.0f, 250.0f, 200.0f, 0.0f, 1000u);
        }
    }
    
    /* IR sensor */
    Sensor_InfraredRead(&g_SensorSystem.Infrared, 2048u);  /* Simulate ADC reading */
    ir_distance = Sensor_InfraredGetDistance(&g_SensorSystem.Infrared);
    if (ir_distance > 0.0f)
    {
        printf("  IR distance: %.1f mm\n", ir_distance);
    }
    
    /* Touch sensor */
    Sensor_TouchUpdate(&g_SensorSystem.Touch, FALSE, 1000u);  /* Simulate touch */
    touch_state = Sensor_TouchIsPressed(&g_SensorSystem.Touch);
    printf("  Touch sensor: %s\n", touch_state ? "Pressed" : "Released");
    
    /* Color sensor */
    Sensor_ColorRead(&g_SensorSystem.Color);
    Sensor_ColorGetRGB(&g_SensorSystem.Color, &red, &green, &blue);
    printf("  Color RGB: (%d, %d, %d)\n", red, green, blue);
    printf("  Illuminance: %.2f lux\n", Sensor_ColorGetLux(&g_SensorSystem.Color));
    
    /* Update filtered sensor values using EASML filters */
    Sensor_SystemUpdate(&g_SensorSystem, 1000u);
    
    printf("Sensor interaction demonstration complete.\n\n");
}

/*
 * Demonstrate communication protocol with CRC
 */
static void Demo_CommunicationTest(void)
{
    Protocol_Packet_Type test_packet;
    Protocol_Packet_Type response_packet;
    Protocol_ServoMoveCmd_Type move_cmd;
    Protocol_SystemStatusResp_Type status_resp;
    Protocol_Status_Type result;
    uint8 test_byte;
    
    printf("=== Communication Protocol Demonstration ===\n");
    
    /* Test 1: Servo move command */
    printf("Testing servo move command...\n");
    move_cmd.ServoIndex = 1u;
    move_cmd.Angle = 450;  /* 45.0 degrees in 0.1 degree units */
    move_cmd.Time = 1500u;
    
    result = Protocol_BuildPacket(&test_packet, CMD_SERVO_MOVE,
                                 (uint8*)&move_cmd, sizeof(move_cmd));
    
    if (result == PROTOCOL_STATUS_OK)
    {
        printf("  Packet built successfully\n");
        printf("  Command: 0x%02X\n", test_packet.Header.Command);
        printf("  Data length: %d bytes\n", test_packet.Header.DataLength);
        printf("  CRC: 0x%04X\n", test_packet.Crc);
        
        /* Verify CRC */
        if (Protocol_VerifyCrc(&test_packet))
        {
            printf("  CRC verification: PASSED\n");
        }
        else
        {
            printf("  CRC verification: FAILED\n");
        }
    }
    
    /* Test 2: System status response */
    printf("\nTesting system status response...\n");
    status_resp.SystemState = 1u;  /* Running */
    status_resp.BatteryVoltage = 7400u;  /* 7.4V */
    status_resp.Temperature = 250u;  /* 25.0°C */
    status_resp.Uptime = 12345u;
    
    uint8 i;
    for (i = 0u; i < 6u; i++)
    {
        status_resp.ServoStatus[i] = 1u;  /* All servos OK */
        status_resp.CurrentAngles[i] = (sint16)(g_RobotState.CurrentAngles.Theta[i] * 10.0f);
    }
    
    result = Protocol_BuildResponse(&response_packet, CMD_RESPONSE_OK,
                                   &status_resp, sizeof(status_resp));
    
    if (result == PROTOCOL_STATUS_OK)
    {
        printf("  Response packet built successfully\n");
        printf("  Total packet size: %d bytes\n", response_packet.TotalLength);
        
        /* Send packet to buffer */
        result = Protocol_SendPacket(&g_ProtocolContext, &response_packet);
        if (result == PROTOCOL_STATUS_OK)
        {
            printf("  Packet queued for transmission\n");
        }
    }
    
    /* Test 3: Byte-by-byte parsing */
    printf("\nTesting packet parsing...\n");
    uint8 test_data[] = {0xAA, 0x55, 0x01, 0x05, 0x01, 0xC2, 0x01, 0xDC, 0x05, 0x12, 0x34};
    
    for (i = 0u; i < sizeof(test_data); i++)
    {
        result = Protocol_ReceiveByte(&g_ProtocolContext, test_data[i], 2000u);
        
        if (result == PROTOCOL_STATUS_OK && 
            g_ProtocolContext.ParseState == PARSE_STATE_WAIT_SYNC1)
        {
            printf("  Complete packet received and parsed\n");
            break;
        }
        else if (result == PROTOCOL_STATUS_CRC_ERROR)
        {
            printf("  CRC error detected\n");
            break;
        }
    }
    
    /* Display statistics */
    uint32 packets_rx, packets_tx, crc_errors, timeout_errors;
    Protocol_GetStats(&g_ProtocolContext, &packets_rx, &packets_tx, &crc_errors, &timeout_errors);
    
    printf("Communication statistics:\n");
    printf("  Packets received: %lu\n", packets_rx);
    printf("  Packets sent: %lu\n", packets_tx);
    printf("  CRC errors: %lu\n", crc_errors);
    printf("  Timeout errors: %lu\n", timeout_errors);
    
    printf("Communication protocol demonstration complete.\n\n");
}

/*
 * Demonstrate EASML math library functions
 */
static void Demo_MathLibraryTest(void)
{
    float32 angle_deg = 45.0f;
    float32 angle_rad;
    float32 sin_val, cos_val, tan_val;
    float32 sqrt_val, pow_val;
    
    printf("=== EASML Math Library Demonstration ===\n");
    
    /* Trigonometric functions */
    angle_rad = angle_deg * 0.0174532925199f;  /* Convert to radians */
    
    sin_val = Mfl_Sin_f32(angle_rad);
    cos_val = Mfl_Cos_f32(angle_rad);
    tan_val = Mfl_Tan_f32(angle_rad);
    
    printf("Trigonometric functions (angle = %.1f°):\n", angle_deg);
    printf("  sin(%.1f°) = %.6f\n", angle_deg, sin_val);
    printf("  cos(%.1f°) = %.6f\n", angle_deg, cos_val);
    printf("  tan(%.1f°) = %.6f\n", angle_deg, tan_val);
    
    /* Mathematical functions */
    sqrt_val = Mfl_Sqrt_f32(25.0f);
    pow_val = Mfl_Pow_f32(2.0f, 8.0f);
    
    printf("\nMathematical functions:\n");
    printf("  sqrt(25) = %.2f\n", sqrt_val);
    printf("  pow(2, 8) = %.0f\n", pow_val);
    
    /* Limiting and rounding */
    float32 test_val = 15.7f;
    float32 limited_val = Mfl_Limit_f32(test_val, 0.0f, 10.0f);
    float32 rounded_val = Mfl_Round_f32(test_val);
    
    printf("\nUtility functions:\n");
    printf("  limit(%.1f, 0, 10) = %.1f\n", test_val, limited_val);
    printf("  round(%.1f) = %.0f\n", test_val, rounded_val);
    
    /* Inverse kinematics using EASML functions */
    printf("\nInverse kinematics calculation:\n");
    float32 target_x = 100.0f, target_y = 150.0f, target_z = 120.0f;
    
    uint8 ik_result = Rob_InverseKinematics(target_x, target_y, target_z, 0.0f, &g_RobotState);
    
    if (ik_result == 0u)
    {
        printf("  Target position: (%.1f, %.1f, %.1f)\n", target_x, target_y, target_z);
        printf("  Calculated joint angles:\n");
        
        uint8 i;
        for (i = 0u; i < 4u; i++)
        {
            printf("    Joint %d: %.2f°\n", i, g_RobotState.TargetAngles.Theta[i]);
        }
    }
    
    printf("EASML math library demonstration complete.\n\n");
}

/*
 * Main control loop demonstrating real-time operation
 */
static void Demo_ControlLoop(void)
{
    static uint32 loop_counter = 0u;
    static float32 target_x = 0.0f;
    static float32 target_y = 200.0f;
    static float32 target_z = 150.0f;
    static boolean direction = TRUE;
    
    uint8 i;
    
    loop_counter++;
    
    /* Generate smooth trajectory every 100 control cycles (2 seconds) */
    if ((loop_counter % 100u) == 0u)
    {
        /* Update target position */
        if (direction)
        {
            target_x += 20.0f;
            if (target_x >= 80.0f)
            {
                direction = FALSE;
            }
        }
        else
        {
            target_x -= 20.0f;
            if (target_x <= -80.0f)
            {
                direction = TRUE;
            }
        }
        
        /* Move to new position */
        Rob_MoveToPosition(&g_RobotState, target_x, target_y, target_z, 0.0f, 1000u);
    }
    
    /* Update PID controllers for smooth servo movement */
    for (i = 0u; i < ROB_SERVO_NUM; i++)
    {
        if (g_RobotState.ServoControl.IsMoving[i])
        {
            Rob_UpdatePID(&g_RobotState, i,
                         g_RobotState.ServoControl.TargetAngle[i],
                         g_RobotState.ServoControl.CurrentAngle[i],
                         0.02f);  /* 20ms control period */
            
            /* Check if servo reached target (simplified) */
            float32 error = Mfl_Abs_f32(g_RobotState.ServoControl.TargetAngle[i] - 
                                       g_RobotState.ServoControl.CurrentAngle[i]);
            if (error < 1.0f)  /* 1 degree tolerance */
            {
                g_RobotState.ServoControl.IsMoving[i] = FALSE;
            }
        }
    }
    
    /* Update sensor system */
    Sensor_SystemUpdate(&g_SensorSystem, loop_counter * DEMO_CONTROL_PERIOD_MS);
    
    /* Print status every 50 cycles (1 second) */
    if ((loop_counter % 50u) == 0u)
    {
        printf("Control loop status (t=%.1fs):\n", (float32)loop_counter * 0.02f);
        printf("  Target position: (%.1f, %.1f, %.1f)\n", target_x, target_y, target_z);
        printf("  Movement complete: %s\n", 
               Rob_IsMovementComplete(&g_RobotState) ? "Yes" : "No");
    }
}
